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ABSTRACT 


This  paper  proposes  the  use  of  Prolog  as  a  rule  based 
specification  language  for  the  coordination  of  multiple 
control  functions  as  required  to  perform  missions  with 
autonomous  underwater  vehicles.  We  first  define  terms 
used  in  this  type  of  control  system  and  show  that  such 
systems  fall  into  the  class  of  ’Hybrid’  controllers  coupling 
discrete  state  /  time  independent  and  continuous  state  / 
continuous  time  elements.  The  design  of  these  systems 
has  received  little  attention,  but,  the  software  architecture 
to  implement  them  is  often  composed  of  three  levels  for 
ease  of  segregation  and  development  of  functionality. 

An  implementation  of  our  controller  on  the  NPS 
Phoenix  vehicle  uses  PROLOG  as  a  rule  based  language 
to  specify  and  execute  the  phases  of  any  mission. 
Embedded  in  the  rule  body  are  functions  that  interface 
with  the  vehicle  to  gather  sensory  data  and  generate 
signals  as  required  to  trigger  transitions  between  control 
functions,  and  to  initiate  commands  for  the  initiation  of 
subsequent  control  functions.  The  importance  of  the  use 
of  PROLOG  is  that  the  same  code  is  used  for  mission 
specification  as  is  used  for  its  execution  thereby 
eliminating  the  question  of  correctness. 

Control  of  a  mission  segment  using  command 
generation  to  simulaneously  drive  the  vehicle  to  a  point 
on  space  and  time  (  to  coincidently  reach  a  given  depth 
and  heading)  is  described  with  experimental  results.  The 


development  of  transitioning  signals  can  be  problematic 
and  is  discussed  alonside  error  recovery  techniques  using 
'guaranteed  phase  completion'. 

INTRODUCTION 

The  human  experience  is  limited  underwater.  Even 
shallow  water  coastal  areas  are  not  well  understood.  As 
an  aid  to  the  development  of  coastal  environmental 
understanding,  new  technology  is  being  aimed  at  using 
(semi  (autonomous  vehicles,  requiring  acoustic 

communications  and  a  degree  of  autonomy  on  the  vehicle 
sufficient  to  maintain  vehicle  task  control.  Building  an 
ever  increasing  level  of  automatic  capability  into  an 
underwater  vehicle  is  of  interest  to  us.  In  particular, 
under  a  new  NSF  grant,  we  are  concerned  with  the  ease 
of  reconfiguration  of  control  software  code  as  missions 
become  more  complex  or  vehicle  capabilities  change.  To 
that  end.  tri-level  software  architectures  are  useful  for 
enabling  control  over  the  resulting  hybrid  system  which 
comprises  discrete  state,  as  well  as  continuous  time  - 
continuous  state  elements.  The  three  levels  (  Strategic, 
Tactical,  and  Execution  levels  in  our  terminology), 
separate  the  control  requirements  into  easily  modularized 
functions  encompassing  logically  intense  discrete  state 
transitioning  using  asynchronously  generated  signals  for 
control  of  the  mission  and  real  time  synchronized 
controllers  that  stabilize  the  vehicle  motion  to  callable 
commands. 

In  our  controller  architecture,  the  Strategic  level  uses 
'PROLOG'  as  a  rule  based  mission  control  specification 
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language.  It's  inference  engine  cycles  through  the 
predicate  rules  to  manage  the  discrete  event  logical 
aspects  of  mission  related  decisions.  It  transitions  states, 
and  generally  develops  the  commands  that  drive  the 
vehicle  through  its  mission.  Error  recovery  procedures 
from  failures  in  the  mission  tasks  or  the  vehicle 
subsystems  are  included  as  transitions  to  'error'  states  that 
ultimately  provide  commands  to  the  servo  level  control 
for  appropriate  recovery  action. 

The  Tactical  level  -  at  the  moment  -  is  set  of  "C" 
language  functions  that  interface  with  the  'PROLOG' 
predicates  and  return  TRUE  /  FALSE  when  called,  and 
which  are  interfaced  to  the  real  time  Execution  level 
controller  using  asynchronous  message  passing  through 
ethernet  sockets. 

The  Execution  level  commands  the  vehicle 
subsystems  to  activate  control  functions  that  correspond 
to  those  commanded.  Communication  from  the  Tactical 
level  to  the  Execution  level  takes  place  through  a  single 
socket.  By  the  design  of  this  hierarchical  control  system, 
the  Tactical  level  runs  asynchronously  and  retains  the 
mission  data  file  and  the  mission  log  file  in  global 
memory.  It  sends  command  scripts  to  the  Execution 
Level  and  requests  data  for  the  evaluation  of  state 
transitions.  The  architecture  is  a  hybrid  between  the  true 
hierarchical  control  of  NASREM  [1]  and  the  purely 
reactive  schemes  of  subsumptionists  [2,  3].  In  this  way, 
control  of  mission  can  be  retained,  while  reacting  to 
unanticipated  events  is  also  enabled. 

In  new  work  with  the  NPS  PHOENIX  -  an 
autonomous  underwater  vehicle,  we  have  extended  the 
flight  control  experiments  that  were  conducted  and 
reported  previously  [4].  We  have  now  developed  the 
thruster  control  behavior  of  the  vehicle.  Experiments 
using  command  generation  to  drive  orthogonal  actions 
have  been  conducted  and  illustrate  herein  both  the  power 
of  sliding  modes  for  control  of  transient  response  and 
command  tracking,  and  the  power  of  PROLOG  for  the 
mission  coordination. 

In  order  to  assist  in  the  subsequent  discussion,  it 
helps  to  define  some  terms  that  have  long  plagued  the 
underwater  robotics  community.  We  offer  the  following: 

Let  the  set  of  all  actuators  available  to  the  vehicle  be 
denoted  by.  A,  the  set  of  sensors  by  S,  and  the  set  of 
continuous  time  states  of  the  vehicle  be  X,  then: 


Definition:  A  Control  Function  (CF),  is  the  use  of  a 
particular  subset,  3.  a  A  of  vehicle  actuators  with  a 
particular  subset,  S.  a  S,  of  the  vehicle  sensor  suite,  to 
both  estimate  a  corresponding  subset  of  the  continuous 
time  continuous  states,  X,  a  X,  of  the  vehicle  and 
drive  a  particular  set  of  continuous  time  error  states,  e, 
to  zero.  The  control  error  is  defined  as  the  difference 
between  a  command  value  and  the  estimated  actual 
motion.  A  control  function  is  analogous  to  the  Robot 
Task  in  [1]  and  employs  an  appropriate  control  law 
linking  the  actuator  commands,  urfkdt),  to  sensory 
output,  y(kdt)  and  commands  r(t). 

CF;  u —f  (y ),r  (t) 

l  l  l  l  l 

Definition:  A  Vehicle  Primative  is  a  linguistic  string 
associated  with  a  particular  Control  Function. 

Definition:  Orthogonal  Control  Functions  are  those  that 
utilize  independent  subsets  of  actuators. 

Definition:  Interacting  Control  Functions  are  those  that 
utilize  part  of  the  same  subset  of  actuators  utilized  by 
other  CF's. 

Definition:  A  Sensor  Based  Reaction,  as  in  a  low 
energy  level  in  the  vehicle,  vehicle  shoaling,  or  any 
obstacle  detection,  is  a  condition  when  a  sensor  value 
sustains  a  critical  value.  If  c  is  the  critical  value,  £  is  a 
small  positive  bound,  and  F  is  a  low  pass  filter,  a  Sensor 
Based  Reaction  occurs  if 

I  F(  y)  —  c  l<e  . 

All  Control  Functions  terminate  at  a  'Termination1. 

Definition:  The  Termination  of  a  control  function 

occurs  either 

1 )  at  a  specified  time, 

2)  when  a  positive  definite  function  P(  ■ )  of  the 

control  error  lies  within  a  prespecified  bound.  If  b ,  is  a 
positive  bound  and  F(  ■ )  is  a  low  pass  filter  the 
termination  condition  is 

F(  P(  e  ))<b 

3)  upon  a  Sensor  Based  Reaction. 


Definition:  A  Behavior  (B),  is  described  as  the 
execution  of  a  particular  sequence  of  CFs  each  driven  to 
a  termination. 

Comment:  The  state  of  performing  a  given  CF  is  a 
"discrete  state"  of  the  system.  The  condition  of  reaching  a 
termination  is  linked  to  the  transition  of  the  discrete  state 
of  the  system  from  one  state  to  another  as  defined  by  the 
system  Behavior  (B).  A  Behavior  can  be  represented  for 
example,  by  a  Petri  Net,  a  finite  state  diagram,  or, 
linguistically,  by  a  set  of  Prolog  rules. 

Definition:  A  Hybrid  Control  System  in  the  context  of 
this  work  is  a  control  system  of  hardware  and  software 
elements  that  is  capable  of  driving  a  vehicle  through  a  set 
of  Behaviors. 

The  definition  of  a  mission  plan  is  now  reduced  to  the 
specification  of  a  sequence  of  Behaviors  (B)  to  be 
conducted  during  each  mission  phase.  These  include  the 
ordered  sequence  of  control  functions  and  their 
termination  conditions. 

The  principle  of  "guaranteed  phase  completion"  is  such 
that  all  control  functions  have  a  termination  so  that  each 
mission  phase,  its  behaviors  and  control  functions,  will 
terminate  -  essentially,  the  mission  plan  will  specify  that 
all  mission  phases  complete  -  either  sucessfully  or  by 
abortion. 

VEHICLE  CONTROL  SYSTEM 

The  control  concepts  presented  are  being  evaluated 
experimentally  using  the  NPS  PHOENIX  vehicle  shown 
in  Figure  1.  It  has  been  recently  outfitted  with  the  tri¬ 
level  controller,  currently  implemented  in  hardware 
using  three  networked  processors,  illustrated  in  Figure  2. 
All  Execution  level  software  is  written  in  'C'  and  runs  on 
a  GESPAC  M68030  processor  in  a  separate  card  cage 
inside  the  boat.  Connected  in  the  same  card  cage  is  an 
ethernet  card  and  an  array  of  real  time  interfacing 
devices  for  communications  to  sensors  and  actuators 
indicated  in  the  details  of  Figure  3.  The  Execution  level 
control  code  containing  a  set  of  functions  in  a  compiled 
module  called  'exec'  is  downloaded  first  and  run  to 
activate  any  mission.  It  starts  the  communication  s  socket 
on  the  GESPAC  side  and  waits  for  the  higher  level 
controller  to  start. 

Strategic  Level 

The  Strategic  level  PROLOG  rules  which  specify  the 
mission  to  be  conducted  are  compiled  and  linked  together 


with  the  supporting  Tactical  level  'C'  language  functions 
into  the  single  executable  process  called 
'Mission_Control',  that  is  run  in  a  SUN  Sparc  4  laptop 
computer  and  linked  through  ethernet  and  a  non- 
blocking  socket  to  the  Gespac  processor.  Upon  starting,  it 
first  opens  the  SUN  side  of  the  communications  socket, 
initiating  the  ethernet  link  between  both  SUN  and 
GESPAC  processors,  then  sending  sequenced  control 
commands  to  the  vehicle.  All  vehicle  control  functions, 
with  the  exception  of  the  transmission  of  sonar  imaging 
data,  communicate  by  message  passing  through  that 
socket. 


Tactical  Level  Software 

A  second  SUN  process  called  the  'Sonar  Manager'  is 
opened  which  runs  asynchronously  in  the  SUN  and  with 
equal  priority  to  the  'Mission_Control'.  This  process  is 
linked  through  a  separate  socket  to  the  GESPAC  for  the 
purpose  of  the  reception  and  handling  of  sonar  imaging 
data.  This  process  is  activated  if  and  when  sonar  is 
activated  by  a  Strategic  level  predicate  call.  The  ’Sonar 
Manager’  captures  data  that  is  sent  out  from  the 
Execution  level  as  soon  as  it  has  been  acquired,  and  then 
processes  and  passes  the  data  to  be  displayed  on  the  IRIS 
Graphics  workstation  for  visualization  purposes.  Tactical 
level  software  is  designed  to  link  with  the  PROLOG  rule 
base,  send  vehicle  primatives  to  the  execution  level 
software  and  process  the  numerical  computaions 
associated  with  computing  the  termination  conditions.  It 
necessarily  requires  the  computation  of  filtered  data,  and 
t  the  present  stage  of  development  performs  computation 
asynchronously.  Time  is  not  critical  as  the 
communication  and  commands  to  receive  data  and 
activate  or  terminate  control  functions  are  designed  to 
change  only  as  needed  and  to  not  influence  the  stability 
of  the  vehicle  motion. 


Execution  Level  Software 

The  structure  of  the  Execution  level  software  is 
illustrated  by  Figure  3  which  indicates  that  it  is 
composed  of  software  at  the  hardware  interface  (software 
drivers)  as  well  as  software  for  vehicle  control.  After 
initialization  of  power  systems  and  sonars,  and  the  basic 
driver  settings,  the  PIA  card  pins  that  control  the  on/off 
feature  of  power  supplies,  thruster  power,  screw  power, 
and  sonar  power,  a  simple  timing  loop  is  entered  and 
reentered  at  a  fixed  update  rate  (in  our  case  0.1  sec.) 
during  which  the  following  takes  place. 


1.  read  the  socket  'A'  for  behavior  based  mode 
command  flags  and  control  set  points, 

2.  read  the  sensors, 

3.  selecting  appropriate  'C'  code  control  functions  for 
computing  and  sending  control  values  to  actuators,  using 
multiple  'case  of  '  checks  for  distinguishing  the 
commands, 

4.  writing  selected  data  to  memory  or  sockets  'A'  or  'B' 
as  appropriate,  and 

5.  checking  time  for  any  time  based  events  and  waiting 
for  the  next  timing  interrupt  to  maintain  integrity  of  the 
digital  control  loop. 

Specific  control  laws  as  built  into  callable  modules  of 
code  are  easily  selected  according  to  the  vehicle 
primatives  sent. 

Vehicle  Primative  Development 

In  previous  work  [5],  waypoint  following  in  a  transit 
phase  of  a  mission  was  demonstrated  in  a  swimming  pool 
test  area  where  competent  control  functions  were 
demonstrated  including  vehicle  primatives 

a)  Forward_Speed_Control 

Control  vehicle  forward  speed  using  stern 

screws. 

b)  Fin_Steering 

Control  vehicle  heading  using  bow  and  stern 
rudders. 

c)  Fin_Depth_Control 

Control  depth  of  the  vehicle  using  bow  and  stern 
planes. 

d)  Waypoint_Following 

Follow  three  dimensional  (X,Y,Z)  waypoints 
using  fins  and  stern  screws. 

e)  Bottom_Following 

Control  the  height  of  the  vehicle  above  the 
bottom. 

These  Control  functions  were  developed  with  a)-c) 
running  simultaneously,  but  subsumed  by  the  guidance 
laws  implemented  in  d);  and.  with  c)  subsumed  by  e). 
The  control  laws  corresponding  to  these  functions  have 
been  implemented  based  on  PD,  and  Sliding  Mode 
methods  as  explained  in  [6] . 

Control  laws  for  these  functions  are  readily 
accomplished  entirely  in  the  Execution  level  using  digital 


control  algorithms  running  at  0.1  sec.  update  rate.  Now, 
however,  new,  more  complex  functions  are  being  enabled 
using  active  control  of  thrusters  and  sonar.  These 
include, 

g)  Submerge_and_Pitch_Control 

Control  vehicle  depth  and  pitch  angle  using 
vertical  thrusters. 

h)  Heading_Control 

Control  vehicle  heading  using  lateral  thrusters. 

i)  Longitudinal_Positional_Control 

Control  longitudinal  position  of  vehicle  from  a 

target. 

j)  Lateral_Positional_Control 

Control  lateral  position  of  vehicle  from  a  target 
using  lateral  thrusters. 

k)  Center_Sonar(Sonar) 

Rotate  sonar  head  'Sonar'  to  ahead  position. 

l)  Ping_Sonar(Sonar,Command) 

Ping  sonar  'Sonar'  using  'Command'. 

m)  Update_Head_Position(Sonar,Command) 

Update  head  position  of  sonar  'Sonar'  based  on 
'Command'. 

n)  Read_Sonar(  Sonar) 

Return  sonar  range  from  'Sonar'. 

o)  Initiate_Filter_For_Sonar_Range 

For  smoothed  range  and  range  rate  (ST  1000 
sonar  only). 

p)  Reinitialize_Filter 

Reset  filter  for  next  data  gathering  /  control 
sequence. 

Note:  Control  functions  g)  through  j)  can  be  implemented 
using  step  input  commands  for  their  activation  or,  for 
more  precise  control  over  transient  behavior,  command 
generators  would  be  used  which  specify  the  desired 
position,  rate,  and  acceleration  of  the  output  as  a  function 
of  time. 

Most  of  these  functions  need  a  given  subset  of  the 
actuator  system  to  be  active  under  the  operation  of  either 
an  open  loop  command  or  a  feedback  control  law.  Some 
of  the  functions  use  orthogonal  sets  of  actuators  and  are 
thus  additive.  Some  use  the  same  actuators  to  control 
different  functions  and  thus  control  laws  may  be  additive. 
This  means,  for  example,  that  vertical  thrusters  may  be 
used  via  control  laws  to  control  depth  as  well  as  pitch, 
and  lateral  thrusters  to  control  heading  as  well  as  lateral 
position  and  side  slip  speed.  In  combination  with 
propulsion  motors,  most  functions  including 
Submerge_and_Pitch_Control,  and  Longitudinal_- 
Position_Control,  as  well  as  Heading_Control,  may  now 
be  commanded.  Heading_Control  and  Submerge_and_ 


Pitch_Control  and  virtually  any  multiple  combination  of 
a)  to  o)  above  that  would  not  cause  a  conflict  of  actuator 
control  or  sensor  usage,  are  performed. 

Activation  of  orthogonal  behaviors  are  instituted 
using  message  passing  that  is  a  way  of  communicating 
between  Tactical  Level  'C'  functions  and  the  real  time 
control  loop  of  the  Execution  Level  control.  At  each  pass 
through  the  control  loop,  a  read  is  made  from  the 
communications  socket  and  a  ladder  check  for  particular 
'case  of  flags  determines  which  set  of  sensors  and 
actuators  and  control  laws  are  to  be  activated  during  the 
computation  cycle.  The  same  technique  is  used  to  flag  the 
activation  of  sonars,  and  filtering  actions,  and  similarly 
for  flags  to  indicate  which  data  stream  is  to  be  written  in 
return. 

Reactive  behavior  in  our  controller  can  be  handled 
inside  the  Execution  level  control  loop  through  command 
overrides  following  a  sensor  read,  as,  for  instance,  a  new 
obstacle  detection  requiring  an  emergency  surface  or 
obstacle  avoidance  (flinch)  response.  At  a  higher  degree 
in  the  Tactical  level,  reactive  error  recovery  can  be 
handled  by  resetting  key  parameters  associated  with 
control  performance  evaluations.  An  example  is  the 
resetting  of  a  control  gain  if  a  particular  function  cannot 
be  stabilized.  Reactive  behavior  is  also  handled  at  the 
Strategic  level  by  transitioning  to  states  that  command  an 
error  recovery  procedure  such  as  to  surface  if,  for 
example,  a  particular  action  is  not  observed  to  be  taken 
after  a  pre-specified  time  out. 

While  the  work  of  [3]  has  developed  GAPPS  rules 
that  are  more  like  our  Strategic  level  rules,  but,  in  the 
end  would  also  provide  mode  commands  to  vehicle 
servos,  our  work  is  developed  around  a  rule  based  conUol 
to  sequence  mission  related  tasks  [7,  8]  according  to  a 
mission  plan  that  could  (if  one  prefers  to  view  it  this 
way)  represent  a  hierarchy  of  state  machines  with 
transitioning  from  one  to  another  as  mission  phases  are 
completed.  The  middle  level  of  our  tri-level  architecture 
is  then  used  to  generate  the  scripts  required  to  produce  in 
the  vehicle  the  requisite  behavioral  action.  The  Tactical 
Level  functions  deal  with  the  interfacing  between 
asynchronous  control  function  commands  and  the  real 
time  computational  control  requirements  of  the  'sense  - 
compute  -  send'  cycle  within  the  Execution  level  vehicle 
motion  control  loop. 

The  behaviors  a)  through  o)  are  now  stably  implemented 
in  the  NPS  PHOENIX  vehicle  through  attention  to 
appropriate  digital  control  loops  in  the  Execution  level. 
In  principle,  once  developed  to  a  satisfactory  point,  the 


Execution  level  controller  of  any  vehicle  would  not 
require  any  change  as  mission  requirements  change. 

COORDINATED  SUBMERGENCE  / 
ROTATIONAL  CONTROL  USING  COMMAND 
GENERATION 


As  part  of  a  joint  mission  to  evaluate  control 
software  architectures  between  US  and  France, 
[wang,rock,lee,  oceans],  it  has  been  decided  to  evaluate 
the  performance  of  the  Nps  Phoenix  in  a  behavior  that 
will  submerge  the  vehicle  to  a  specified  depth  at  a 
speified  rate  according  to  a  command  function.  A  similar 
function  will  describe  the  required  heading  and  heading 
rate  so  that  the  attainment  of  the  new  final  position  and 
heading  will  occur  at  a  defined  final  time.  The 
performance  of  this  type  of  maneuver  with  land  robotic  is 
relativle  easy  but  the  performance  underwater  is  not 
known  until  now. 

In  this  secion  we  will  describe  the  command 
generators  used,  and  show  the  control  performnac 
obtained  with  sliding  mode  control  functions  executing 
simulataneously. 

Vehicle  Model  for  Submergence 

The  vehicle  dynamics  in  submergence  using  both  the 
bow  and  stern  vertical  thrusters  can  be  described  by  the 
following  differential  equation  for  the  continuous  time, 
continuous  state  evolution: 

Mz(t)  +  bi(t)\i(t)\  =2Zprop(t)  +  FB(t)+5f(t)  (1) 
where 

M  =m  +  m  ( added  ) 
Zprop(t)=av(t)\v(t)\ 

and  m  ( added )  is  the  vertical  added  mass,  a  is  a 
coefficient  relating  the  square  of  the  thruster  motor 
voltage,  v(  t ) ,  to  the  force  developed.  FB(  t )  is  the 
unmatched  buoyancy  which  varies  within  some  bound 
but  which  on  any  day  can  be  either  positive  or  negative, 
and  is  unknown,  b  is  the  coefficient  of  square  law  drag, 
and  5/Y  t )  describes  an  upper  bound  on  vehicle/model 
parameter  mismatch. 


The  command  generator  for  the  submerge  motion  is 
taken  from 

/  ''com  *  Z. com  >  Z com  ]  ''0  ’  '■/  ’  ^ max  ’  ^0  ’  ^ f  ) 

where  a  fith  order  zero  jerk  profile  has  been  chosen  so 
that  the  maximum  acceleration  and  the  bandwidth 
capacity  of  the  vehicle  is  not  overly  exceeded  and  zg ,  Tg 
is  the  initial  depth  and  starting  time  while  Zf  ,  1/  is  the 
final  desired  depth  and  time  at  the  end  of  the  maneuver. 


v(t)  =  ± 

+  r\tanh(<5(  t)/§  )) 


(5) 


+  ^-(bz( i )\z( f  )\- FB(t)-Sf(  t)) 


where 


a(  t  )=z(  t)  +  Xz(  t  )  +  \z(t  )dt . 


(6) 


Vehicle  Model  for  Rotation 

The  vehicle  dynamics  for  rotation  about  the  body- 
fixed  z-axis  (yaw)  using  both  the  bow  and  stern  lateral 
thrusters  can  be  described  by  the  following  differential 
equation  for  the  continuous  time,  continuous  state 
evolution: 


IZV  (t)  +  M|/( t)\y(t)\  =  2Nprop {t)  +5f(t)  (7) 


where 


L  =/„+/„(  added ) 
Nprop(t)=av(t)\v(t)\ 


Figure  4.  Normalized  Command  Generator  Profiles 

The  sliding  mode  control  law  for  submerging  is 
given  by 


v(t)  =  ± 


M 

—(  ^com  +^z(t)  +T\tanh( a(t)/<\>  )) 

2a 


1 


(2) 


+  —0>z(t)\z(t)\-FB(t)-5f(t)) 


where  the  sliding  surface  is 

a(t)  =  t(t)  +  1£(t)  (3) 

and  the  tracking  errors  are  defined  as 


and  ( added )  is  the  added  inertia  about  the  z-axis.  a 
is  a  coefficient  relating  the  square  of  the  thruster  motor 
voltage,  v(  t) ,  to  the  force  developed,  b  is  the  coefficient 
of  square  law  drag,  and  8  f{  t )  describes  an  upper  bound 
on  vehicle/model  parameter  mismatch. 

The  command  generator  for  rotation  is  taken  from 

/  V  i  /mi  ’  V  com  ’  V  com  7  =G(V|/0,V|// 

’  ®max  ’  ^  ^  ’ 

where  y  g ,  Tg  is  the  initial  heading  and  starting  time 
while  v  f,  Tf  is  the  final  desired  heading  and  time  at 
the  end  of  the  maneuver  and  is  also  fifth  order  with  zero 
jerk. 


z(t)  =  z(t)-zcom(t) 
z(t)  =  z(t)-Zcom(t). 


(4) 


The  sliding  mode  control  law  for  rotational  control  is 
given  by 


Using  integral  control  the  command  for  voltage  is 


v(t)  =  ± 


~^(  Vcom  +  M(t)  +r\tanh(  oft  )/<$>)) 
2a 


+  —(by(t)\v(t)\-FB(t)-5f(t)) 


(8) 


1 1  alone  has  shown  to  be  unsatisfactory.  The  signal  can 
be  smoothed  by  filtering  a  through  a  first  order  digital 
filter  of  the  form 


o  ftk+D  =  m  +  (]~  e'm  (12) 


where  the  sliding  surface  is 

o(t)=ys/(t)+'hs/(t) 
and  the  tracking  errors  are  defined  as 

\j 7(t)=y(t)-\\tcom(t) 
\p(t)=y(t)-ycom(t). 


where  a  ,  is  the  filtered  form  of  a  ,  %  is  the  time 
constant  of  the  filter,  and  T  is  the  sampling  time.  The 
condition  for  transition  can  be  shown  diagrammatically 
,q  .  in  Figure  xx,  which  indicates  that  the  signal  for 
transition,  s ,  is  1  (TRUE)  for  a  <  oj0  or  0  (FALSE) 

for  a  i  >  a  fQ.  Other  dynamic  error  and  time  based 
signals  are  computed  similarly. 

(10) 

COMMAND  TRACKING  PERFORMANCE 


Transition  Criteria 

Most  control  phase  transitions  of  the  Phoenix  are 
event  based,  meaning  that  a  certain  set  of  criteria  must  be 
met  in  order  for  a  transition  to  occur.  A  common 
example  of  this  is  when  a  position  set  point  is  sent  to  the 
vehicle  controllers  and  reached.  A  method  of 
determining  whether  the  vehicle  has  indeed  reached  this 
point  must  be  programmed  into  the  control  logic. 
Measuring  the  position  error  alone  and  declaring  the 
maneuver  complete  when  this  error  is  small  is  not 
sufficient.  This  is  because  the  vehicle  could  be 
overshooting  the  commanded  position  and  simply 
passing  through  the  set  point.  Therefore,  not  only  must 
the  position  error  be  small  but  the  rate  error  must  also  be 
small.  This  dual  criteria  can  be  expressed  mathematically 
as  a  positive  definite,  linear  combination  of  the  position 
error  e  and  the  position  rate  error  §■ .  We  use, 

^k=we\ek\  +  wi\ek\  (11) 

where  we  and  w&  are  positive  weights  for  the  position 
and  rate  errors  respectively.  This  equation  allows  a 
minimum  value  of  a  ,  denoted  a  0 ,  to  be  specified 
defining  a  threshold  for  the  combination  of  errors  which 
can  be  set  relatively  large  when  precision  control  is  not 
required  or  low  for  extremely  precise  positioning.  Once 
a  drops  below  a  0 ,  the  maneuver  is  declared  complete 
and  a  transition  to  the  next  control  phase  may  occur. 

When  noisy  sensors  are  used,  the  noise  prevents  a 
from  settling  enough  to  determine  an  accurate 
measurement  for  the  transition,  and  the  use  of  Equation 


The  following  experiment  was  performed  in  the  NPS 
hover  tank  which  measures  6.0  by  6.0  meters  square  and 
1.8  meters  deep.  During  execution  all  pertinent  data  was 
collected,  including  depth,  depth  rate,  heading,  heading 
rate,  thruster  motor  speed,  etc.  The  experiment  required 
the  vehicle  to  simultaniously  submege  and  rotate  to  a 
predetermined  depth  of  1  meter  and  a  heading  of  180 
degrees.  It  was  specified  that  the  final  depth  and  heading 
both  be  reached  at  60  seconds  from  the  beginning  of  the 
manuever.  This  was  accomplished  using  command 
generators  for  both  control  modes  with  integral  control 
for  depth  using  an  anti-reset  windup  saturation  of  0.45 
m-sec.  The  following  tables  give  the  values  used  in  the 
vehicle  control  law  were  the  vehicle  mass,  drag  and 
thruster  gains  are  from  [Kevin]  and  the  controller  gains 
were  obtained  from  computer  simulation  results. 


Table  1.  Paramters  for  Submergence  Control 


Parameter 

V  alue 

Unit 

m 

194.88 

Kg 

m  (added) 

194.88 

Kg 

b 

1378.18 

Kg/m 

a 

0.018 

N/V2 

X] 

0.400 

rad/sec 

X  £ 

0.040 

rad/sec2 

T) 

0.030 

B 

1 

4> 

0.061 

m/sec 

Table  2.  Paramters  for  Heading  Control 


Parameter 

Value 

Unit 

'zz 

53.60 

Kg-m2 

lzz  (added) 

53.60 

Kg-m2 

b 

74.86 

Kg-m2 

a 

0.008 

N-m/V2 

X 

0.200 

rad/sec 

n 

0.200 

rad/sec2 

<l> 

0.200 

rad/sec 

Figure  6.  Normalized  Heading  and  Heading  Rate 
Response 


Figures  5  and  6  show  the  normalized  time  responses 
for  depth/depth  rate  and  heading/heading  rate 
respectively.  The  vehicle  was  trimmed  to  be  neutrally 
buoyant  on  the  surface  and  the  depth  and  heading  was  set 
to  zero  at  this  point.  The  depth  response  tracks  very  well 
until  the  steady  state  region  where  an  overshoot  occurs. 
This  is  due  to  the  vehicle  becoming  "heavy"  at  depth 
from  hull  compression,  although  the  error  is  quickly 
corrected  by  the  integral  action  of  the  controller.  Since  no 
disturbance  was  present  in  rotation,  the  heading  response 
shows  an  extremely  precise  tracking  performance  with 
virtually  no  error. 

The  depth  rate  does  track  the  command  but  is  very 
noisy  due  to  discretization  noise  from  the  A/D  converter 
associated  with  the  depth  cell  and  the  subsequent  rate 
estimation  from  this  signal.  Although  the  signal  is  far 
from  clean,  the  tracking  performance  is  not  adversely 
affected.  The  heading  rate  measured  from  the  onboard 
rate  gyroscope  shows  a  definate  tracking  error  and  is  due 
to  a  non-zero  bias  in  the  unit. 

It  can  be  seen  from  Figure  7  that  the  depth  and 
heading  is  simultaneously  controlled  except  for  the  small 
depth  overshoot  at  the  end  of  the  maneuver. 


Figure  5.  Normalized  Depth  and  Depth  Rate  Response 


Figure  7.  Normalized  Depth  vs.  Heading  Response 


CONCLUSION 

The  conclusion  of  our  work  to  date  has  indicated  that 
complex  behavior  can  be  readily  coordinated  through 
Strategic  level  rules,  that  are  easily  modified.  These  act 
as  state  transitioning  mechanisms  and  the 
communication  through  Tactical  level  software  to  the 
Execution  level  controllers  is  a  simple  but  convenient 
way  of  commanding  competent  functions  of  the  vehicle. 
The  design  of  well  behaved  control  laws  and  functions  at 
the  Execution  level  is  essential  as  a  primary  part  of  the 
design  an  is  effected  through  careful  attention  to  the 
digital  control  loop  design.  Human  interfacing  within  the 
controller  can  take  place  at  any  level. 

The  independent  coordination  of  sonar  for  range 
finding  on  a  bearing,  or  for  imaging  over  a  particular 
sector  of  bearings  is  needed  to  derive  motion  commands 
for  the  vehicle.  Smooth  vehicle  motion  can  be  achieved 
in  an  underwater  environment  free  from  current  and 
wave  action  provided  that  attention  is  given  to  the 
processing  of  the  sonar  data,  but  time  delays  in 
processing  sonar  data  is  a  difficult  problem  to  handle  and 
is  still  under  research.  We  would  anticipate,  however, 
that  in  the  future,  sonar  based  relative  navigation  without 
the  use  of  LBL  could  be  possible  in  structured  or  feature 
rich  scenes.  The  work  is  continuing. 
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Figure  1  Outline  Of  The  NPS  PHOENIX  Vehicle 


Figure  2  Outline  Of  The  Phoenix  Networked  Controller 
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Diagram  of  the  Software  /  Hardware  Interface 
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Figure  3  The  Structure  Of  The  Execution  Level  Software 


